/*
 * RT-Thread Secure
 *
 * Copyright (c) 2021, Shanghai Real-Thread Electronic Technology Co., Ltd.
 *
 * All rights reserved.
 */
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>

struct rk3568_frac_pdata
{
    rt_ubase_t frac_reg;
    rt_ubase_t gate_reg;
    int shift;
    int invert;
};
typedef struct rk3568_frac_pdata rk3568_frac_pdata_t;

/* 计算 m&n 的最大公约数 */
rt_inline rt_uint32_t calc_gcd(rt_uint32_t m, rt_uint32_t n)
{
    rt_uint32_t t;
    while (m > 0) { if(n > m) { t = m; m = n; n = t; } m -= n; }
    return n;
}

static void set_enable(struct rt_clk *clk, rt_bool_t enable)
{
    rk3568_frac_pdata_t *pdata = (rk3568_frac_pdata_t *)clk->pdata;
    rt_uint32_t value = 0;

    if(enable)
    {
        value = (pdata->invert ? 0x0 : 0x1) << pdata->shift;
    }
    else
    {
        value = (pdata->invert ? 0x1 : 0x0) << pdata->shift;
    }

    setreg(pdata->gate_reg, value);
}

static rt_bool_t get_enable(struct rt_clk *clk)
{
    rk3568_frac_pdata_t *pdata = (rk3568_frac_pdata_t *)clk->pdata;

    if(readl(pdata->gate_reg) & (0x1 << pdata->shift))
    {
        return pdata->invert ? RT_FALSE : RT_TRUE;
    }

    return pdata->invert ? RT_TRUE : RT_FALSE;
}

static void set_rate(rt_clk_t *clk, rt_uint64_t parent_rate, rt_uint64_t rate)
{
    rk3568_frac_pdata_t *pdata = (rk3568_frac_pdata_t *)clk->pdata;
    rt_uint64_t cdiv;
    rt_uint32_t mult, div;

    if(rate == 0)
    {
        rate = parent_rate;
    }

    cdiv = calc_gcd(parent_rate, rate);
    div = (parent_rate / cdiv) & 0xffff;
    mult = (rate / cdiv) & 0xffff;
    writel((mult << 16) | (div << 0), pdata->frac_reg);
}

static rt_uint64_t get_rate(rt_clk_t *clk, rt_uint64_t parent_rate)
{
    rk3568_frac_pdata_t *pdata = (rk3568_frac_pdata_t *)clk->pdata;
    rt_uint32_t value = readl(pdata->frac_reg);
    return (parent_rate * ((value >> 16) & 0xffff)) / ((value >> 0) & 0xffff);
}

static rt_uint64_t get_round_rate(rt_clk_t *clk, rt_uint64_t parent_rate, rt_uint64_t rate)
{
    rt_uint64_t cdiv;
    rt_uint32_t mult, div;

    cdiv = calc_gcd(parent_rate, rate);
    div = (parent_rate / cdiv) & 0xffff;
    mult = (rate / cdiv) & 0xffff;

    return parent_rate * mult / div;
}

static struct rt_clk_ops ops =
{
    .set_enable = set_enable,
    .get_enable = get_enable,
    .set_rate = set_rate,
    .get_rate = get_rate,
    .get_round_rate = get_round_rate,
};

rt_clk_t *rt_clk_rk3568_frac_register(const char *name, const char *parent, rt_ubase_t frac_reg,
    rt_ubase_t gate_reg, rt_uint32_t shift, rt_bool_t invert)
{
    rt_clk_t *clk = RT_NULL;
    rk3568_frac_pdata_t *pdata = RT_NULL;

    RT_ASSERT(name && parent);

    if(rt_strlen(name) == 0 || rt_strlen(parent) == 0)
    {
        clk = RT_NULL;
        goto _fail;
    }

    clk = rt_clk_create(name, &ops, 1, sizeof(rk3568_frac_pdata_t), 0);
    if(!clk)
    {
        goto _fail;
    }

    clk->parents[0] = rt_clk_find(parent);
    pdata = (rk3568_frac_pdata_t *)clk->pdata;
#if defined(RT_USING_SMART)
    pdata->frac_reg = (rt_ubase_t)rt_ioremap((void *)frac_reg, 4096);
    pdata->gate_reg = (rt_ubase_t)rt_ioremap((void *)gate_reg, 4096);
#else
    pdata->frac_reg = frac_reg;
    pdata->gate_reg = gate_reg;
#endif /* RT_USING_SMART */
    pdata->shift = shift;
    pdata->invert = invert;

    if(rt_clk_register(clk) != RT_EOK)
    {
        goto _fail;
    }

    return clk;

_fail:
    if(clk)
    {
        rt_free(clk);
    }

    return RT_NULL;
}

rt_err_t rt_clk_rk3568_frac_unregister(rt_clk_t *clk)
{
    rt_err_t ret = RT_EOK;

    ret = rt_clk_unregister(clk);
    if(!ret)
    {
        return ret;
    }

    rt_clk_delete(clk);

    return RT_EOK;
}
